#include "FastCluster.h"
#include <cmath>

namespace cluster
{

FastCluster::FastCluster() :isGrid_(false), squaredDist_(0)
{
    
}

FastCluster::~FastCluster()
{

}

vecClusterRes_t FastCluster::cluster(const vecClusterObject_t &vecObject)
{
    vecClusterRes_t vecCluster;
    isGrid_ = false;
    clusterCall(vecObject, vecCluster);
    return vecCluster;
}

vecClusterRes_t FastCluster::clusterFor2D(const vecClusterObject_t &vecObject, const double maxDist)
{
    //1.
    if (maxDist < 0.0001)
    {
        return {};
    }

    isGrid_      = true;
    squaredDist_ = maxDist * maxDist;

    //2.
    mapGridObject_t mapObject;
    for (auto &obj : vecObject)
    {
        mapObject[calcGridIndex(obj, maxDist)].push_back(obj);
    }

    //3.
    vecClusterRes_t vecCluster, vecRes;
    clusterInSingleGrid(mapObject, vecCluster);

    //4.
    clusterInMulGrid(mapObject);

    //5.
    for (auto &res : vecCluster)
    {
        if (!res->vecObject.empty())
        {
            vecRes.push_back(res);
        }
    }

    //6.
    return vecRes;
}

bool FastCluster::isMatch([[maybe_unused]] const std::shared_ptr<ClusterObject_t> &a, [[maybe_unused]] const std::shared_ptr<ClusterObject_t> &b) const
{
    return true;
}

void FastCluster::clusterCall(const vecClusterObject_t &vecObject, vecClusterRes_t &vecRes) const
{
    //1.
    std::shared_ptr<ClusterRes_t> cluster;
    vecClusterRes_t vecCluster;
    for (auto &obj : vecObject)
    {
        //1.1
        cluster = nullptr;
        for (auto &src : vecCluster)
        {
            if (!src->vecObject.empty())
            {
                if (isMatchForCluster(obj, src))
                {
                    if (cluster != nullptr)
                    {
                        mergeCluster(src, cluster);
                    }
                    else
                    {
                        cluster = src;
                    }
                }
            }
        }

        //1.2
        if (cluster == nullptr)
        {
            cluster = std::make_shared<ClusterRes_t>(static_cast<uint32_t>(vecCluster.size()));
            vecCluster.push_back(cluster);
        }

        //1.3
        cluster->vecObject.push_back(obj);
    }

    //2.
    for (auto &res : vecCluster)
    {
        if (!res->vecObject.empty())
        {
            for (auto &obj : res->vecObject)
            {
                obj->cluster = res;
            }

            vecRes.push_back(res);
        }
    }
}

void FastCluster::clusterInSingleGrid(const mapGridObject_t &mapObject, vecClusterRes_t &vecCluster) const
{
    vecClusterRes_t vecRes;
    for (auto &it : mapObject)
    {
        //1.
        clusterCall(it.second, vecRes);

        //2.
        for (auto &cluster : vecRes)
        {
            for (auto &obj : cluster->vecObject)
            {
                obj->cluster = cluster;
            }
        }

        //3.
        std::copy(vecRes.begin(), vecRes.end(), std::back_inserter(vecCluster));
        vecRes.clear();
    }
}

void FastCluster::clusterInMulGrid(const mapGridObject_t &mapObject) const
{
    std::size_t num(0);
    vecClusterObject_t vecDstObj;
    setClusterRes_t setSrc, setDst;
    GridInt_t si, ei, sj, ej, ci, cj;
    std::shared_ptr<ClusterRes_t> cluster;
    std::set<std::pair<ClusterObject_t*, ClusterObject_t*>> setMatch;
    
    for (auto &it : mapObject)
    {
        //1.
        ci = it.first.first;
        cj = it.first.second;
        si = ci - 1;
        ei = ci + 2;
        sj = cj - 1;
        ej = cj + 2;

        //2.
        vecDstObj.clear();
        for (GridInt_t i = si; i != ei; ++i)
        {
            for (GridInt_t j = sj; j != ej; ++j)
            {
                if (i != ci || j != cj)
                {
                    auto it2 = mapObject.find({i,j});
                    if (it2 != mapObject.end())
                    {
                        std::copy(it2->second.begin(), it2->second.end(), std::back_inserter(vecDstObj));
                    }
                }
            }
        }

        //3.
        for (auto &src : it.second)
        {
            auto srcCluster = src->cluster.lock();
            for (auto &dst : vecDstObj)
            {
                auto dstCluster = dst->cluster.lock();
                if (srcCluster != dstCluster)
                {
                    //3.1
                    setMatch.insert(getKey(src, dst));
                    if (num == setMatch.size())
                    {
                        continue;
                    }

                    //3.2
                    num = setMatch.size();
                    if (distIsOK(src, dst) && isMatch(src, dst))
                    {
                        mergeCluster(dstCluster, srcCluster);
                    }
                }
            }
        }
    }
}

bool FastCluster::isMatchForCluster(const std::shared_ptr<ClusterObject_t> &obj, const std::shared_ptr<ClusterRes_t> &cluster) const
{
    if (isGrid_)
    {
        for (auto &src : cluster->vecObject)
        {
            if (distIsOK(src, obj) && isMatch(src, obj))
            {
                return true;
            }
        }
    }
    else
    {
        for (auto &src : cluster->vecObject)
        {
            if (isMatch(src, obj))
            {
                return true;
            }
        }
    }

    return false;
}

bool FastCluster::distIsOK(const std::shared_ptr<ClusterObject_t> &a, const std::shared_ptr<ClusterObject_t> &b) const
{
    return std::pow(a->x - b->x, 2) + std::pow(a->y - b->y, 2) < squaredDist_;
}

void FastCluster::mergeCluster(std::shared_ptr<ClusterRes_t> &src, std::shared_ptr<ClusterRes_t> &dst) const
{
    //1.
    for (auto &obj : src->vecObject)
    {
        obj->cluster = dst;
    }

    //2.
    std::copy(src->vecObject.begin(), src->vecObject.end(), std::back_inserter(dst->vecObject));

    //3.
    src->vecObject.clear();
}

std::pair<GridInt_t, GridInt_t> FastCluster::calcGridIndex(const std::shared_ptr<ClusterObject_t> &obj, double dist) const
{
    GridInt_t i = static_cast<GridInt_t>(std::round(obj->x / dist));
    GridInt_t j = static_cast<GridInt_t>(std::round(obj->y / dist));
    return {i,j};
}

std::pair<ClusterObject_t*, ClusterObject_t*> FastCluster::getKey(const std::shared_ptr<ClusterObject_t> &a, const std::shared_ptr<ClusterObject_t> &b) const
{
    return a.get() < b.get() ? std::make_pair(a.get(), b.get()) : std::make_pair(b.get(), a.get()) ;
}

}